#!/usr/bin/python
# -*- coding: UTF-8 -*-

'''
Author: LDW0331 2061278380@qq.com
Date: 2024-03-29 09:41:04
LastEditors: LDW0331 2061278380@qq.com
LastEditTime: 2024-04-03 17:07:54
FilePath: /hrg_bee_dev_v3/src/network_module/ros_app_flask_bridge/parser/cmdvel_parser.py
Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
'''

import rospy
from geometry_msgs.msg import Twist

from cln_msgs.msg import HardwareState

from tool import Log_Parser
log = Log_Parser()

class CmdvelParser:
    def __init__(self):
        self.__node_name = "CmdvelParser"

    def InitParser(self, node_name):
        '''
            初始化
        '''
        ## node name配置
        self.__node_name = node_name

        self.__charge_line = 0 # 充电线反馈状态 1 插入 0拔出

        ##发布topic
        self.__cmdvel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1, tcp_nodelay=True)

        ## 订阅话题
        rospy.Subscriber("/hardware_state",HardwareState,self.HardwareState_Callback,queue_size=1)  #20230726-dw.liu 
    
    def GetNodeName(self):
        '''
            获取节点名称
        '''
        return self.__node_name

    #--------------------------------------#
    #******#  ROS topic publication #******# 
    #--------------------------------------#
    def PublishCmdvelTopic(self,msg):
        '''
            方控指令话题发布
        '''
        self.__cmdvel_pub.publish(msg)


    #--------------------------------------#
    #******#  ROS topic Subscriber #******# 
    #--------------------------------------#
    def HardwareState_Callback(self, msg_data = HardwareState):
        '''
        订阅硬件状态话题
        create-20230726-dw.liu
        '''
        self.__charge_line = msg_data.charge_line # 充电线反馈状态 1 插入 0拔出
        
  
      

    #-----------------------------------#
    #******#  Interface function #******# 
    #-----------------------------------#
    '''
        速度控制
    '''
    def SetCmdvel(self, data):
        ret_dat = {};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data:
                raise Exception("Request data error!")      
            msg = Twist()
            msg.linear.x = float(data["linear_x"])
            msg.linear.z = 1 # 与硬件模块约定，速度格式中线速度的z轴置位1，代表方控控制
            msg.angular.z = float(data["angular_z"]) 

            if self.__charge_line == 0:
                # 当充电线拔出的时候，方控起作用
                self.PublishCmdvelTopic(msg)
            
        except Exception as error:

            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[CmdvelParser]Failed to set Cmdvel: {}".format(str(error))
            log.LogError(self.GetNodeName(),err_msg)  
            # rospy.logerr(err_msg)
        finally:
            return ret_dat